#!/usr/bin/python3
# coding=utf8
import sys
import json
sys.path.append('/home/pi/MasterPi/')
import time
import HiwonderSDK.Board as Board
from pid.PidController import PidController

if sys.version_info.major == 2:
    print('Please run this program with python3!')
    sys.exit(0)



CMD_TAKE_OFF = 1
CMD_STOP = 2
CMD_EMERGENCY_STOP = 3
CMD_HEART_BIT = 11
CMD_ALL_ANGLE = 14
CMD_AUTO_ON = 20
CMD_AUTO_OFF = 21
CMD_SERVO_BACK=4
CMD_SERVO_FORWARD=5

MOTOR_1 = 1
MOTOR_3 = 3
MOTOR_2 = 2
MOTOR_4 = 4

ATTITUDE_UPDATE_RATE_DIVIDER = 2
IMU_UPDATE_DT = 0.01

class Controller:
    def __init__(self,gyro) -> None:
        self.__isTakingOff = False
        self.__attitudeCounter = 0
        self.__PID = PidController(2.0, 0.4, 0.6)
        self.__gyro = gyro
        self.__expectYaw = 0
        self.__expectThrottle = 0
        self.__auto = False
        Board.setPWMServoPulse(MOTOR_1, 1000, 0)
        Board.setPWMServoPulse(MOTOR_2, 1000, 0)
        pass
    
    def motorLimit(self,throttle_out):
        if(throttle_out>2000):
            throttle_out = 2000
        
        if(throttle_out<1000):
            throttle_out=1000
        return throttle_out

    def __stopMotor(self):
        Board.setPWMServoPulse(MOTOR_1, 1000, 0)
        Board.setPWMServoPulse(MOTOR_2, 1000, 0)

    
    def __move(self,throttle,yaw):
        # throttle : 0 - 100
        # yaw   -30, 30
        # 电机输出 1000 - 2000


        throttle_out = 1000 + throttle * 10

        throttle_out = self.motorLimit(throttle_out)

        sub = yaw * 15
        half_sub = sub / 2

        motor1_throttle = throttle_out + half_sub
        motor2_throttle = throttle_out - half_sub

        
        
        if(motor1_throttle>2000):
            motor1_throttle = 2000
            motor2_throttle = motor1_throttle - sub
        if(motor2_throttle>2000):
            motor2_throttle = 2000
            motor1_throttle = motor2_throttle + sub

        if(motor1_throttle<1000):
            motor1_throttle=1000
            motor2_throttle = motor1_throttle - sub
        
        if(motor2_throttle<1000):
            motor2_throttle=1000
            motor1_throttle = motor2_throttle + sub

        #print("motor1="+str(motor1_throttle))
        #print("motor2="+str(motor2_throttle))
        
        Board.setPWMServoPulse(MOTOR_1, int(motor1_throttle), 0)
        Board.setPWMServoPulse(MOTOR_2, int(motor2_throttle), 0)



    def emergencyStop(self):
        self.__isTakingOff = False
        self.__stopMotor()
            
 

    def exec(self,cmd,data):
        
        if(cmd==CMD_ALL_ANGLE):
            # 0 : 油门大小（0-100）
            # 1 ：偏航角度（-30,30）
            if(self.__isTakingOff):
                if(self.__auto==False):
                    self.__move(int(data[0]),int(data[1]))
                else:
                    self.__expectThrottle=int(data[0])
                    self.__expectYaw=int(data[1])
        elif(cmd==CMD_TAKE_OFF):
            self.__isTakingOff = True
        elif(cmd==CMD_STOP):
            self.emergencyStop()
        elif(cmd==CMD_EMERGENCY_STOP):
            self.emergencyStop()
        elif(cmd==CMD_AUTO_ON):
            self.__auto=True
        elif(cmd==CMD_AUTO_OFF):
            self.__auto=False
        elif(cmd==CMD_SERVO_BACK):
            Board.setPWMServoPulse(MOTOR_3, 500, 0)
            Board.setPWMServoPulse(MOTOR_4, 2500, 0)
        elif(cmd==CMD_SERVO_FORWARD):
            Board.setPWMServoPulse(MOTOR_3, 2500, 0)
            Board.setPWMServoPulse(MOTOR_4, 520, 0)
            
    def autorun(self):
        while(True):
            time.sleep(IMU_UPDATE_DT)
            if(self.__isTakingOff==False or self.__auto==False):
                continue
            
            gyroData  = self.__gyro.getGryoData()
        
            speedDesired = 0
            #angle control
            if(self.__attitudeCounter >= ATTITUDE_UPDATE_RATE_DIVIDER):
                speedDesired = self.__PID.pidUpdate(self.__expectYaw - gyroData[2],IMU_UPDATE_DT*ATTITUDE_UPDATE_RATE_DIVIDER)
                self.__attitudeCounter=0
            self.__attitudeCounter+=1
            #speed control
            yawout = self.__PID.pidUpdate(speedDesired - gyroData[5],IMU_UPDATE_DT)
            
            
            #motor output
            
            motor1_throttle = self.motorLimit(self.__expectThrottle + yawout)
            motor3_throttle = self.motorLimit(self.__expectThrottle - yawout)
            
            
            Board.setPWMServoPulse(MOTOR_1, int(motor1_throttle), 0)
            Board.setPWMServoPulse(MOTOR_2, int(motor3_throttle), 0)
            
        
if __name__ == '__main__':
    for i in range(3):
        Board.setPWMServoPulse(MOTOR_1, 2450, 0)
        time.sleep(2)
        Board.setPWMServoPulse(MOTOR_1, 500, 0)
        time.sleep(2)
        
        
        